/*
 该代码为误差测试代码
 通过反复同一运动轨迹测量最终的累计误差
 */
long sub = 200,a=360,x=1080,t = 7,s = 360,v = 720;
int times = 0,tt = 1;
float ti;
volatile boolean isinitial = 0;
boolean w = 0;
#include <AccelStepper.h>
AccelStepper stepx(1, 9, 8);
void h();
// Define a stepper and the pins it will use
void setup()
{  
    Serial.begin(9600);
    Serial.println("");
    Serial.print(F("请输入电机细分:"));
    while (!Serial.available()) {}
    sub = Serial.parseInt();
    Serial.println(sub);
    Serial.print(F("请输入加速度:"));
    while (!Serial.available()) {}
    a = Serial.parseInt();
    Serial.println(a);
    Serial.print(F("请输入最大速度:"));
    while (!Serial.available()) {}
    x = Serial.parseInt();
    Serial.println(x);
    Serial.print(F("请输入每次前进的距离:"));
    while (!Serial.available()) {}
    s = Serial.parseInt();
    Serial.println(s);
    Serial.print(F("请输入分几次走完:"));
    while (!Serial.available()) {}
    tt = Serial.parseInt();
    Serial.println(tt);
    Serial.print(F("请输入反复的次数:"));
    while (!Serial.available()) {}
    t = Serial.parseInt();
    Serial.println(t);
    Serial.print(F("请输入初始化速度:"));
    while (!Serial.available()) {}
    v = Serial.parseInt();
    Serial.println(v);
    stepx.setMaxSpeed(sub * (v+360) / 360);
    stepx.setSpeed(sub * v / 360);
//    Serial.println(stepx.speed());
    while(!Serial.available());
    while(Serial.read()>0);
    Serial.println("let's go");
    attachInterrupt(1, h, FALLING);
}

void loop()
{
    if(Serial.available())
    {
      if(Serial.read() == 'p')
      {
        stepx.setCurrentPosition(stepx.currentPosition());
        Serial.println("急停");

      }
      else if(Serial.read()=='x' && !isinitial)
      {
        stepx.setSpeed(Serial.parseInt());
      }
      while(Serial.read()>0);
    }
    if (!isinitial)
    {
        stepx.runSpeed();
    }
    else
    {
      if(!w)
      {
        w= 1;
       while(Serial.read()>0);
       while(!Serial.available());
       while(Serial.read()>0);
       stepx.setAcceleration((float)(a * sub / 360));
       stepx.setMaxSpeed((float)(sub * x / 360));
       Serial.println("开始工作");
       ti = millis();
      }
      else{
                // If at the end of travel go to the other end
        if(times < 2*t){
        if(stepx.currentPosition() == 0){
          for(int i = 1;i <=tt;i++){
           stepx.moveTo((long)(s/tt*i * sub / 360));
           stepx.runToPosition();
           delay(100);
          }
        }
        else if(stepx.currentPosition() == (long)(s * sub / 360))
        {
          for(int i = tt-1 ;i>=0;i--){
            stepx.moveTo((long)(s/tt*i * sub / 360));
            stepx.runToPosition();
            delay(100);
          }

        }
        times++;
        if(times%2==0)
        {
          Serial.print("当前是第");
          Serial.print(times/2);
          Serial.println("次");
          Serial.println("输入任意字符继续前行");
          while(!Serial.available());
          while(Serial.read()>0);
          Serial.println("继续前行");
        }
    }
      }
   }
   if(times == 2*t)
   {
    Serial.println((float)((millis()-ti)/1000));
    while(1);
    }
}

void h()
{
    stepx.setCurrentPosition(0);
    Serial.println("中断已经发生");
    isinitial = 1;  
    detachInterrupt(1);
}
